package com.neuronrobotics.aim.robot;

import java.io.IOException;
import java.io.InputStream;
import java.lang.Math;
import java.util.ArrayList;

import javax.swing.JFrame;
import javax.swing.JOptionPane;
import javax.xml.parsers.DocumentBuilder;
import javax.xml.parsers.DocumentBuilderFactory;
import javax.xml.parsers.ParserConfigurationException;

import org.w3c.dom.Document;
import org.w3c.dom.Element;
import org.w3c.dom.Node;
import org.w3c.dom.NodeList;
import org.xml.sax.SAXException;

import Jama.Matrix;

//import com.neuronrobotics.aim.gui.MainGUI;
//import com.neuronrobotics.sdk.common.Log;
import com.neuronrobotics.aim.gui.EventResponseTable;
import com.neuronrobotics.aim.robot.xml.MRIXmlFactory;
import com.neuronrobotics.sdk.pid.IPIDControl;
import com.neuronrobotics.sdk.pid.IPIDEventListener;
import com.neuronrobotics.sdk.pid.PIDConfiguration;
import com.neuronrobotics.sdk.pid.PIDEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;

/**
 * The Class ProstateRobotKinematicModel.
 * @author Hao Su
 */
public class ProstateRobotKinematicModel implements IKinematicsModel,IPIDEventListener {	
	//TODO list
	//pitch unit: mm/degree
	/** The pitch. */
	private double pitch=2;
	
	// parameters related to {Robot} frame in {Base} frame
	/** L1 the link length of the lower bar(80mm in total) */
	public final double L1=40.0;
	// L2 the initial link length of the bottom bar
	/** The L2. */
	public final double L2=69.03;
	
	//************************************************ joint limits
	/** The x stage lower limit. */
	public final double stageXAxisLowerLimit=-15;
	/** The x stage upper limit. */
	public final double stageXAxisUpperLimit=15;

	/** The Y stage lower limit. */
	public final double stageYAxisLowerLimit=0;
	/** The Y stage upper limit. */
	public final double stageYAxisUpperLimit=L1/pitch;
	
	/** The Z stage lower limit. */
	public final double insertionAxisLowerLimit=0;
	/** The Z stage upper limit. */
	public final double insertionAxisUpperLimit=60;
	//************************************************ joint limits
	
	
	//************************************************ Cartesian space XYZ limits
	/** Cartesian x axis lower limit. */
	public final double xAxisLowerLimit=-15;
	/** The x stage upper limit. */
	public final double xAxisUpperLimit=15;

	/** Cartesian Y  lower limit. */
	public final double yAxisLowerLimit=143;
	/** The Y stage upper limit. */
	public final double yAxisUpperLimit=170;
	
	/** Cartesian Z axis lower limit. */
	public final double zAxisLowerLimit=251;
	/** Cartesian Z axis upper limit. */
	public final double zAxisUpperLimit=310;
	//************************************************ joint limits
	
	/** The x offset robotin base. */
	double xOffsetRobotinBase=0;
	
	/** The y offset robotin base. */
	double yOffsetRobotinBase=101.69;
	
	/** The z offset robotin base. */
	double zOffsetRobotinBase=250.45;
	
//	// the initial height of {Robot}T in {Base} from SolidWokrs
//	public final double Y0=151.133;
//	// the initial Z direction offset of {Robot} in {Base} from SolidWorks
//	public final double Z01=0.0;
//	// the initial needle length in {Robot} measure from real needle
//	public final double Z02=120.0;
	////////////////////////////////////////////////////////////

	/** The z offset insertion. */
private final double zOffsetInsertion=0;
	
	// Linear encoder convention ratio: 1 inch has 500 ticks==0.02032
	/** The encoderticks2mm. */
	public final double encoderTicks2mm= 0.0508;
	// Rotary encoder convention ratio: 1 round has 1250 ticks=360/1250=0.288, pitch is 2mm/rotation=2mm/360degree
	// TODO list: check this ratio
	/** The encoderticks2degree. */
	public final double encoderTicks2degree= pitch/360*0.288;

	
	/** The device. */
	@SuppressWarnings("unused")
	private IPIDControl device =null;
	
	ArrayList<PIDConfiguration> conf= new ArrayList<PIDConfiguration>();

	/** The configurations. */
	ArrayList<LinkConfiguration> configurations= new ArrayList<LinkConfiguration>();
    // constructor to assign the values of transformation matrix
	/**
     * Instantiates a new prostate robot kinematic model.
     */
    public ProstateRobotKinematicModel(){
		loadConfig();
	}
	
	/**
	 * Instantiates a new prostate robot kinematic model.
	 *
	 * @param rotationMatrix the rotation matrix
	 * @param position the position
	 */
	public ProstateRobotKinematicModel(double rotationMatrix[][],double position[]){
		
		loadConfig();
	}
	
	/**
	 * Load config.
	 */
	private void loadConfig(){
		//Load and configure XML
		InputStream config = MRIXmlFactory.getDefaultConfigurationStream("ProstateRobotHao4axis.xml");
		DocumentBuilderFactory dbFactory = DocumentBuilderFactory.newInstance();
	    DocumentBuilder dBuilder;
	    Document doc = null;
	    try {
			dBuilder = dbFactory.newDocumentBuilder();
			doc = dBuilder.parse(config);
			doc.getDocumentElement().normalize();
		} catch (ParserConfigurationException e) {
			throw new RuntimeException(e);
		} catch (SAXException e) {
			throw new RuntimeException(e);
		} catch (IOException e) {
			throw new RuntimeException(e);
		}
		//System.out.println("Parsing File...");
		NodeList nList = doc.getElementsByTagName("link");
		for (int temp = 0; temp < nList.getLength(); temp++) {
			//System.out.println("Leg # "+temp);
		    Node nNode = nList.item(temp);
		    if (nNode.getNodeType() == Node.ELEMENT_NODE) {
		    	configurations.add(new LinkConfiguration((Element) nNode));
		    }else{
		    	//System.out.println("Not Element Node");
		    }
		}
	}
	public PIDConfiguration getLinkCurrentConfiguration(int chan){
		return conf.get(chan);
	}
	public void setLinkCurrentConfiguration(int chan,PIDConfiguration c){
		conf.set(chan, c);
	}
	@Override
	public void setDevice(IPIDControl device) {
		device.addPIDEventListener(this);
		this.device=device;
		//Log.enableDebugPrint(true);
		for(LinkConfiguration c:configurations){
			try{
				PIDConfiguration tmpConf = device.getPIDConfiguration(c.getIndex());
				tmpConf.setKP(c.getKP());
				tmpConf.setKI(c.getKI());
				tmpConf.setKD(c.getKD());
				//conf.setEnabled(true);
				tmpConf.setEnabled(false);
				tmpConf.setInverted(c.isInverted());
				tmpConf.setAsync(true);
				System.out.println("Axis #"+c.getIndex()+" Configuration:");
				printConfig(tmpConf);
				conf.add(tmpConf);
				setLinkCurrentConfiguration(c.getIndex(),tmpConf);
				

			}catch(Exception ex){
				System.err.println("Configuration #"+c.getIndex()+" failed!!");
				//ex.printStackTrace();
			}
		}
		System.out.println("Finished setting up device");
	}

	public void printConfig (PIDConfiguration conf){
		System.out.println("Enabled "+ conf.isEnabled());
		System.out.println("Inverted "+ conf.isInverted());
		System.out.println("Async  "+ conf.isAsync());
		System.out.println("Kp  "+ conf.getKP());
		System.out.println("Ki  "+ conf.getKI());
		System.out.println("Kd  "+ conf.getKD());	
		System.out.println("*********************");
	}

	 /**
 	 * This function is to calculate desired tip position in Robot coordinate,
 	 * given registration matrix and desired needle position in RAS.
 	 * 
 	 * Input are two matrices {Tip} T in {IMG} and {Z} T in {IMG}. The constant matrix {Base} T in {Z} is also used,
 	 * then output the desired tip position in {Base}
 	 *
 	 * @param tableDesiredNeedleRAS the table desired needle ras
 	 * @param tableZFrameRegistration the table z frame registration
 	 * @param tableBaseinZFrameCalibration the table basein z frame calibration
 	 * @return 3x1 Cartesian Space Vector (unit: in mm)
 	 */
	
	public double[] calculateDesiredTipPosition(EventResponseTable tableDesiredNeedleRAS, EventResponseTable tableZFrameRegistration, 
			EventResponseTable tableBaseinZFrameCalibration){
		// {Tip} T in {IMG}
		Matrix matrixDesiredNeedleRAS = new Matrix(tableDesiredNeedleRAS.getTableData());
		// {Z} T in {IMG}
		Matrix matrixZFrameRegistration = new Matrix(tableZFrameRegistration.getTableData());
		// {Base} T in {Z}
		Matrix matrixBaseinZFrame = new Matrix(tableBaseinZFrameCalibration.getTableData());
		 
		// a temporary matrix 
		 Matrix martixTemp = new Matrix(4, 4);
		 martixTemp = matrixZFrameRegistration.times(matrixBaseinZFrame);
		// a temporary matrix to store the inverse of the product of two matrices.
		 Matrix matrixTempInverse= martixTemp.inverse();
		 // {Tip} T in {Base}
		 Matrix matrixTipInBase = matrixTempInverse.times(matrixDesiredNeedleRAS);
		 double[][] arrayTipInZ = matrixTipInBase.getArray();		 
		 //extract the 3x1 vector of needle tip position in {Base} frame
		 double[] cartesianSpaceVector ={arrayTipInZ[0][3],arrayTipInZ[1][3],arrayTipInZ[2][3]};
		 // return 3x1 tip position in {Base} 
		 return cartesianSpaceVector; 	 
	}
	

	 /**
 * Forward kinematics.
 *
 * @param jointSpaceVector the joint space vector
 * @return 3x1 vector in Cartesian space, unit mm
 */
	public double[] forwardKinematics(double[] jointSpaceVector) {
		double[] cartesianSpaceVector=new double[3];
		//from prismatic joint qX (unit mm) to x 
		cartesianSpaceVector[0]=jointSpaceVector[1];
		//from joint qRotation (unit degree) generates y direction height to y
		//cartesianSpaceVector[1]=L2-2*Math.sqrt(Math.pow(L1, 2)-Math.pow(4/7*(jointSpaceVector[1]-Y0), 2));
		cartesianSpaceVector[1]=2*Math.sqrt(L1*L1-0.25*(L2-jointSpaceVector[0]*pitch)*(L2-jointSpaceVector[0]*pitch))+yOffsetRobotinBase;
		//from prismatic joint qZInsertion insertion DOF to z
		cartesianSpaceVector[2]=jointSpaceVector[2]+zOffsetRobotinBase+zOffsetInsertion;
		return cartesianSpaceVector;
		}
	
	/**
	 * Inverse kinematics.
	 *
	 * @param cartesianSpaceVector the Cartesian space vector
	 * @return 6x1 vector in Cartesian space, in mm
	 * @throws Exception 
	 */ 
	public double[] inverseKinematics(double[] cartesianSpaceVector) throws Exception {
		double[] jointSpaceVector=new double[6];
		//from x to prismatic joint qRotation (unit mm) 
		jointSpaceVector[0]=(L2-2*Math.sqrt(L1*L1-0.25*(cartesianSpaceVector[1]-yOffsetRobotinBase)*(cartesianSpaceVector[1]-yOffsetRobotinBase)))/pitch;
		//from y to joint qX (unit degree)
		jointSpaceVector[1]=cartesianSpaceVector[0];			
		//from z to prismatic joint qZInsertion insertion DOF
		jointSpaceVector[2]=0;
		jointSpaceVector[3]=cartesianSpaceVector[2]-zOffsetRobotinBase-zOffsetInsertion;
		// q rotation is set to 0
		jointSpaceVector[4] =0;
		
		//exception handling
		if (Double.isNaN(jointSpaceVector[1])){
			throw new Exception("NaN result!");
		}	
		return jointSpaceVector;
		}
	
 	/**
 	 * convert joint ticks to physical joint values (in mm and degree).
 	 *
 	 * @param jointSIUnits for prostate robot, it is 6x1 vector (unit: ticks)
 	 * @return the double[]
 	 * for prostate robot, it is 6x1 vector (unit: mm or degree)
 	 */
	public double[] jointSIUnits2EncoderTicks(double[] jointSIUnits) {
		double[] encoderTicks=new double[6];
		
		jointSIUnits = doubleRound2ThreeDigits(jointSIUnits);		
		// qX, linear motor
		encoderTicks[0]=jointSIUnits[0]/encoderTicks2mm;
		// qY, rotary motor
		encoderTicks[1]=jointSIUnits[1]/0.004;

		encoderTicks[2]=jointSIUnits[2]/encoderTicks2mm;
		//qInsertion, linear motor
		encoderTicks[3]=jointSIUnits[2]/encoderTicks2mm;
		//qRotation, rotary motor, not used
		encoderTicks[4]=jointSIUnits[3]/0.004;		
		return encoderTicks;
		}
	
	 private double[] doubleRound2ThreeDigits(double[] longDigitsDouble) {
		 for (int i = 0; i < longDigitsDouble.length; i++) {
				longDigitsDouble[i] = Math.round(longDigitsDouble[i] * 1000) / 1000.00;
			}
			return longDigitsDouble;
	}

	/**
 	 * convert joint ticks to physical joint values (in mm and degree).
 	 *
 	 * @param jointEncoderTicks for prostate robot, it is 6x1 vector (unit: ticks)
 	 * @return the double[]
 	 * for prostate robot, it is 6x1 vector (unit: mm or degree)
 	 */
	public double[] encoderTicks2JointSIUnits(int[] jointEncoderTicks){
		/*if (jointEncoderTicks.length!=6)
		{
			System.err.println("Must be 6 dimension vector");
		}*/
		// print the length of channels (8 in DyIO)
		//System.out.println(jointEncoderTicks.length+ "  Channels in the hardware");
		
		// define a 6 dimension double array
		double[] jointSIUnits =new double[5];
		// qX, linear motor
		jointSIUnits[0]=jointEncoderTicks[0]*encoderTicks2mm;
		// qY, rotary motor
		jointSIUnits[1]=jointEncoderTicks[1]*encoderTicks2degree;
		// qZ, linear motor, not used
		jointSIUnits[2]=jointEncoderTicks[2]*encoderTicks2mm;
		//qInsertion, linear motor
		jointSIUnits[3]=jointEncoderTicks[3]*encoderTicks2degree;
		jointSIUnits[4]=jointEncoderTicks[4]*encoderTicks2degree;
		
		return jointSIUnits;
	}

	
	/**
	 * check if the testValue is out of range of the [min, max] bound.
	 *
	 * @param testValue the test value
	 * @param max the max bound
	 * @param min the min bound
	 * @return true, if test value is withinRange
	 */
	public boolean withinBound( double testValue, double min, double max){
		if ((testValue>= max) || (testValue <= min))
		{
			return false;
		}
		else
			return true;		
	}
	
	/**
	 * Check joint range.
	 *
	 * @param jointSpaceVector the joint space vector
	 */
	public boolean checkJointSpaceRange(double[] jointSpaceVector ) {
		boolean isOutRange=false;
		String msg = "";
		
		if (!withinBound(jointSpaceVector[0],stageYAxisLowerLimit,stageYAxisUpperLimit)){
			msg = msg+ "Y axis is out of range\n";
			isOutRange=true;
		}
		
		if (!withinBound(jointSpaceVector[1],stageXAxisLowerLimit,stageXAxisUpperLimit)){
			msg = msg+ "X axis is out of range\n";
			isOutRange=true;
		}
		
		if (!withinBound(jointSpaceVector[3],insertionAxisLowerLimit,insertionAxisUpperLimit)){
			msg = msg+ "Insertion axis is out of range\n";
		}
		
		if (isOutRange==true){
			JOptionPane.showMessageDialog(new JFrame(), msg, "Range Warning",
		            JOptionPane.WARNING_MESSAGE);
		}
		else{
			JOptionPane.showMessageDialog(new JFrame(), "Joint calculated and within range", "Sucessful calculation",
		            JOptionPane.INFORMATION_MESSAGE);
		}
		return isOutRange;
	}
	
	/**
	 * Check joint range.
	 *
	 * @param cartesianSpaceVector the joint space vector
	 */
	public boolean checkCartesianSpaceRange(double[] cartesianSpaceVector ) {
		boolean isOutRange=false;
		String msg = "";
		
		if (!withinBound(cartesianSpaceVector[0],xAxisLowerLimit,xAxisUpperLimit)){
			msg = msg+ "Cartesian X axis is out of range\n";
			isOutRange=true;
		}
		
		if (!withinBound(cartesianSpaceVector[1],yAxisLowerLimit,yAxisUpperLimit)){
			msg = msg+ "Cartesian Y axis is out of range\n";
			isOutRange=true;
		}
		
		if (!withinBound(cartesianSpaceVector[2],zAxisLowerLimit,zAxisUpperLimit)){
			msg = msg+ "Cartesian Z axis is out of range\n";
		}
		
		if (isOutRange==true){
			JOptionPane.showMessageDialog(new JFrame(), msg, "Workspace Warning",
		            JOptionPane.WARNING_MESSAGE);
		}
		else{
			JOptionPane.showMessageDialog(new JFrame(), " Within workspace", "Workspace",
		            JOptionPane.INFORMATION_MESSAGE);
		}
		return isOutRange;
	}
	
	@Override
	public void onPIDEvent(PIDEvent e) {
		// TODO Auto-generated method stub
		
	}
	
	@Override
	public void onPIDLimitEvent(PIDLimitEvent e) {
		// TODO Auto-generated method stub
		
	}
	
	@Override
	public void onPIDReset(int group, int currentValue) {
		// TODO Auto-generated method stub
		
	}

	public double getKP(int axis) {

		return configurations.get(axis).getKP();
	}
	public double getKI(int axis) {

		return configurations.get(axis).getKI();
	}
	public double getKD(int axis) {

		return configurations.get(axis).getKD();
	}
}
